//使用一个独立的arduino作为从模块控制抓取机构。pin D2接收来自主机的消息，每一次高电位进行一次抓取/释放
/* void Catch()  //夹取物品
 * void unCatch() //释放物品
 * 
 */
#include <Servo.h>
Servo ArmR;
Servo ArmL;
Servo HandR;
Servo HandL;
int val;
int cat;
void setup() {
  // put your setup code here, to run once:
  CarInit();
}

void CarInit()
{
  Serial.begin(115200);
  ArmL.attach(8);
  ArmR.attach(7);
  HandR.attach(11);
  HandL.attach(12);
  pinMode(2,INPUT);

  SetArm(120);
  SetHand(90);
  delay(500);
  val = 0;
  cat = 0;
}

void loop() {
  // put your main code here, to run repeatedly:
  
  val = digitalRead(2);
  if(val == 1)
  {
    Serial.print("Start");
    if(cat == 0)
    {
      Catch();
      cat = 1;
      delay(5000);
    }
    else if(cat == 1)
    {
      unCatch();
      cat = 0;
      delay(4000);
    }
    val = 0;
  }
  Serial.print(val);
  Serial.print(" ");
  Serial.println(cat);
  delay(50);
}

void SetArm(int Ang)  //60 - 120 (低-高)
{
  ArmR.write(Ang-5);
  ArmL.write(180-Ang);
}

void SetHand(int Ang)  //90- 130(开-合)
{
  HandR.write(Ang);
  HandL.write(180-Ang);
}

void Catch()
{
  SetArm(120);
  SetHand(90);  //高举，打开
  delay(1500);
  
  SetArm(60);
  delay(1000);
  SetHand(130); //放低，抓取
  delay(1000);

  SetArm (120); //举起
  delay(1000);
}

void unCatch()
{
  SetArm(60);
  delay(1000);
  SetHand(90); //放低，释放
  delay(1000);

  SetArm(120); //举起
  delay(1000);
}
